/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.grt192.mechanism.breakaway;

import com.grt192.actuator.GRTJaguar;
import com.grt192.actuator.GRTServo;
import com.grt192.core.Command;
import com.grt192.core.GRTRobot;
import com.grt192.core.Mechanism;
import com.grt192.core.Sensor;
import com.grt192.sensor.GRTAxisCamera;
import com.grt192.sensor.GRTSwitch;
import edu.wpi.first.wpilibj.templates.MainRobot;


/**
 *
 * @author GRTstudent
 */
public class KickerOmega extends Mechanism{

    public static final double KICK_SPEED = -1.0;

    public static final double SWITCH_HIT = Sensor.FALSE;//false or true?

    private boolean kicking;

    public KickerOmega(GRTJaguar jaguar,
            GRTSwitch limitSwitch) {
        //start jaguar, servo, limitSwitch
        kicking = false;
        jaguar.start();
        limitSwitch.start();

        //add actuators of jaguar, limitSwitch, and servo with string tags
        addActuator("Motor", jaguar);
        addSensor("LimitSwitch", limitSwitch);
 
    }

    public boolean hitSwitch() {
        return getSensor("LimitSwitch").getState("State") == SWITCH_HIT;
    }

    public void setSpeed(double speed) {
        getActuator("Motor").enqueueCommand(new Command(speed));
    }

    public void forward(){
        setSpeed(-KICK_SPEED);
        kicking = false;
                //Forward is in the opposite direction of kicking
    }
    public void reverse(){
        setSpeed(KICK_SPEED);
        kicking = false;
        //Reverse is in the same direction of kicking
    }

    public void kick() {
        setSpeed(KICK_SPEED);
        kicking = true;
    }

    public void stopKick() {
        setSpeed(0);
        kicking = false;
    }

    public boolean isKicking() {
        return kicking;
    }



}